/*
angle_encode.c
offer angle encode driver function realizy
designed by lunanting

----------------------------
2020-12-28 11:43:50
create file
----------------------------
*/

#include "angle_encode.h"
#include "../location/joint.h"
#include "math.h"
#include "stdlib.h"
#include "string.h"
#include "aos/kv.h"

int xp_angleEncode_write_reginster(Type_Angle_Encode_Def * const handler, uint16_t addr, uint16_t *data, uint8_t len);
int xp_angleEncode_read_reginster(Type_Angle_Encode_Def * const handler, uint16_t addr, uint16_t read_len, uint8_t *rec_data, uint8_t *rec_len);

int xp_angleEncode_read_angle(Type_Angle_Encode_Def * const handle);
void xp_angleEncode_show_list(void);


Type_Angle_Encode_Def angle_encode_list[] = {
	
	[0].sens_type = SENS_SE100T40,
	[0].is_connect = false,
	[0].com.port = 1,
#ifdef DRYER
	[0].com.speed = 19200,
#else
	[0].com.speed = 115200,
#endif
	[0].com.slave_id = 1,
	[0].resolution = 4096,
	[0].offset = 0,
	
	[1].sens_type = SENS_BRT38R0M,
	[1].is_connect = false,
	[1].com.port = 1,
#ifdef DRYER
	[1].com.speed = 19200,
#else
	[1].com.speed = 115200,
#endif
	[1].com.slave_id = 2,
	[1].resolution = 4096,
	[1].offset = 0,
	
	[2].sens_type = SENS_BRT38R0M,
	[2].is_connect = false,
	[2].com.port = 1,
#ifdef DRYER
	[2].com.speed = 19200,
#else
	[2].com.speed = 115200,
#endif
	[2].com.slave_id = 3,
	[2].resolution = 4096,
	[2].offset = 0,
};

//获取角度传感器offset参数字典
char *xp_angleEncode_get_offset_strKey(uint8_t slave_id)
{
	static char Buf[20];

	sprintf(Buf, "SensId_%d_offset", slave_id);
	return Buf;
}
//获取角度传感器is_reverse_direct参数字典
char *xp_angleEncode_get_reverse_strKey(uint8_t slave_id)
{
	static char Buf[20];

	sprintf(Buf, "SensId_%d_reverse", slave_id);
	return Buf;
}

//储存角度传感器参数
int xp_angleEncode_save_params(char *pKeyStr, int32_t val)
{
	char buf[10];
	
	if (NULL != pKeyStr){
		sprintf(buf, "%d", val);
		uint8_t tryTimes = 3;
		while(tryTimes--){
			if( 0 != aos_kv_set(pKeyStr, buf, strlen(buf), 1) ){
				print("Save error. <%s>\r\n", pKeyStr);
			}else{
				return 0;
			}
		}
	}
	return -1;
}

//读取角度传感器参数
int32_t xp_angleEncode_read_params(char *pKeyStr)
{
	char buf[10];
	int len = sizeof(buf);
	
	if (NULL != pKeyStr){
		uint8_t tryTimes = 3;
		while(tryTimes--){
			if (0 != aos_kv_get(pKeyStr, buf, &len)){
				print("Read error. <%s>\r\n", pKeyStr);
			}else{
				return atoi(buf);
			}
		}
	}
	return -1;
}


int xp_angleEncode_init(void)
{
	int ret = 0;
	
	for (uint8_t i = 0; i < sizeof(angle_encode_list) / sizeof(Type_Angle_Encode_Def); i++){
		
		//读取角度传感器offset参数
		if ( -1 != (ret = xp_angleEncode_read_params( xp_angleEncode_get_offset_strKey( angle_encode_list[i].com.slave_id ) )) ){
			angle_encode_list[i].offset = ret / 1000.0f;
		}else{
			angle_encode_list[i].offset = 0;
			xp_angleEncode_save_params(xp_angleEncode_get_offset_strKey( angle_encode_list[i].com.slave_id ), 0);
		}
		//读取角度传感器is_reverse_direct参数
		if ( -1 != (ret = xp_angleEncode_read_params( xp_angleEncode_get_reverse_strKey( angle_encode_list[i].com.slave_id ) )) ){
			if (ret){
				angle_encode_list[i].is_reverse_direct = true;
			}else{
				angle_encode_list[i].is_reverse_direct = false;
			}
		}else{
			angle_encode_list[i].is_reverse_direct = false;
			xp_angleEncode_save_params(xp_angleEncode_get_reverse_strKey( angle_encode_list[i].com.slave_id ), 0);
		}
		
		//初始化端口modbus
		ret = xp_modbus_init(angle_encode_list[i].com.port, angle_encode_list[i].com.speed, MB_PAR_NONE, 100);
		
		if(0 != ret){
			println("Angle sens <id:%d> modbus init fail ! ret = %d", angle_encode_list[i].com.slave_id, ret);
			return ret;
		}else if ( 0 == (ret = xp_angleEncode_read_angle(&angle_encode_list[i])) ){
			angle_encode_list[i].is_connect = true;
			println("Angle sens <id:%d> init success, Angle:%.2f", angle_encode_list[i].com.slave_id, angle_encode_list[i].angle);
		}else{
			println("Angle sens <id:%d> modbus init success, but read data failed ! ret = %d", angle_encode_list[i].com.slave_id, ret);
		}
	}
	xp_angleEncode_show_list();
    return ret;
}
//读取角度编码器单圈值,后面增加编码器型号在这里增加一个case
int xp_angleEncode_read_single_count(Type_Angle_Encode_Def * const handle)
{
	int ret = 0;
	
	switch ((int)handle->sens_type){
		case SENS_BRT38R0M:
			ret = xp_angleEncode_read_reginster(handle, ADDR_BRT38R0M_SINGLE_COUNT, 1, handle->com.rec_data, &handle->com.rec_len);
			if (0 == ret){
				handle->single_count = (handle->com.rec_data[0] << 8) | handle->com.rec_data[1];
			}
		break;
		
		case SENS_SE100T40:
			ret = xp_angleEncode_read_reginster(handle, ADDR_SE100T40_SINGLE_COUNT, 1, handle->com.rec_data, &handle->com.rec_len);
			if (0 == ret){
				handle->single_count = (handle->com.rec_data[0] << 8) | handle->com.rec_data[1];
			}
		break;
	}
	return ret;
}
//读取角度编码器多圈值,后面增加编码器型号在这里增加一个case
int xp_angleEncode_read_mult_count(Type_Angle_Encode_Def * const handle)
{
	int ret = 0;
	
	switch ((int)handle->sens_type){
		case SENS_BRT38R0M:
			ret = xp_angleEncode_read_reginster(handle, ADDR_BRT38R0M_MULT_COUNT, 1, handle->com.rec_data, &handle->com.rec_len);
			if (0 == ret){
				handle->mult_count = (handle->com.rec_data[0] << 8) | handle->com.rec_data[1];
			}
		break;
		
		case SENS_SE100T40:
			//不支持多圈
		break;
	}
	return ret;
}
//原始值根据角度偏移,旋转方向转换成物理角度
int xp_angleEncode_conver_angle(Type_Angle_Encode_Def * const handle)
{
	if (0 != handle->resolution && handle->offset > -360.0f && handle->offset < 360.0f){
		float angle = (float)handle->single_count / handle->resolution * 360.0f;
		if (handle->is_reverse_direct){
			angle = 360.0f - angle;
		}
		handle->angle = fmod(angle + 360.0f - handle->offset, 360.0f);
	}else{
		return -1;
	}
	return 0;
}
//读取角度传感器物理角度
int xp_angleEncode_read_angle(Type_Angle_Encode_Def * const handle)
{
	int ret = xp_angleEncode_read_single_count(handle);
	if (0 == ret){
		handle->is_connect = true;
		return xp_angleEncode_conver_angle(handle);
	}else{
		handle->is_connect = false;
	}
	return ret;
}
//阻塞式通过id读取角度值
int xp_angleEncode_read_angle_by_id(uint8_t id, float *angle)
{
	int ret = -5;

	if ( id < sizeof(angle_encode_list) / sizeof(Type_Angle_Encode_Def) ){
		ret = xp_angleEncode_read_angle(&angle_encode_list[id]);
		if (0 == ret){
			*angle = angle_encode_list[id].angle;
		}
	}
	return ret;
}
//非阻塞式通过id获取角度值,如果传感器未连接,返回1
float xp_angleEncode_get_angle_by_id(uint8_t id, float *angle)
{
	if ( id < sizeof(angle_encode_list) / sizeof(Type_Angle_Encode_Def) && angle_encode_list[id].is_connect ){
		*angle = angle_encode_list[id].angle;
		return 0;
	}
	return 1;
}
//获取某个编码器是否异常: 0-无异常，1-异常
int xp_angleEncode_is_error(uint8_t id)
{
	if ( id < sizeof(angle_encode_list) / sizeof(Type_Angle_Encode_Def) ){
		if ( angle_encode_list[id].is_connect ){
			return 0;
		}else{
			return 1;
		}
	}else{
		println("angleEncode id %d NOT exist!!!", id);
		return 0;
	}
}

int xp_enc_read_all_encode(void)
{
	int ret = xp_angleEncode_read_reginster(&angle_encode_list[0], 0x00, 3, angle_encode_list[0].com.rec_data, &angle_encode_list[0].com.rec_len);
	if (0 == ret){
		angle_encode_list[0].single_count = (angle_encode_list[0].com.rec_data[0] << 8) | angle_encode_list[0].com.rec_data[1];
		angle_encode_list[1].single_count = (angle_encode_list[0].com.rec_data[2] << 8) | angle_encode_list[0].com.rec_data[3];
		angle_encode_list[2].single_count = (angle_encode_list[0].com.rec_data[4] << 8) | angle_encode_list[0].com.rec_data[5];
		
		for (uint8_t i = 0; i < 3; i++){
			if (0xFFFF == angle_encode_list[i].single_count){
				angle_encode_list[i].is_connect = false;
				println("Angle Encode <%d> error\r\n", i);
			}else{
				angle_encode_list[i].is_connect = true;
			}
		}
		xp_angleEncode_conver_angle(&angle_encode_list[0]);
		xp_angleEncode_conver_angle(&angle_encode_list[1]);
		xp_angleEncode_conver_angle(&angle_encode_list[2]);
	}
	return ret;
}
int xp_enc_cacl_offset(uint8_t index, float tarAngle)
{
	if ( index < sizeof(angle_encode_list) / sizeof(angle_encode_list[0]) && angle_encode_list[index].is_connect ){
		angle_encode_list[index].offset = 0;
		xp_angleEncode_conver_angle(&angle_encode_list[index]);
		
		float sens_angle;
		xp_angleEncode_get_angle_by_id(index, &sens_angle);
		angle_encode_list[index].offset = fmod(sens_angle - tarAngle + 360, 360);
		xp_angleEncode_conver_angle(&angle_encode_list[index]);
		xp_angleEncode_get_angle_by_id(index, &sens_angle);
        
		int32_t temp = (int32_t)(angle_encode_list[index].offset * 1000);
		xp_angleEncode_save_params(xp_angleEncode_get_offset_strKey( angle_encode_list[index].com.slave_id ), temp);
		print("Config sensor %d <id:%d> offset:%.2f  currAngle is %.2f\r\n", index, angle_encode_list[index].com.slave_id, angle_encode_list[index].offset, sens_angle);

	}else if (sizeof(angle_encode_list) / sizeof(angle_encode_list[0]) == index){
		angle_encode_list[index - 1].offset = 0;
		float sens_angle = 0, sum = 0;
		
		for(uint8_t i = 0; i < sizeof(angle_encode_list) / sizeof(angle_encode_list[0]); i++){
            xp_angleEncode_conver_angle(&angle_encode_list[index - 1]);
			xp_angleEncode_get_angle_by_id(index - 1, &sens_angle);
			sum += sens_angle;
		}

		angle_encode_list[index - 1].offset = -fmod(tarAngle - sum + ROBOT_CENTER_ANGLE - 180 + 720, 360);

		int32_t temp = (int32_t)(angle_encode_list[index - 1].offset * 1000);
		xp_angleEncode_save_params(xp_angleEncode_get_offset_strKey( angle_encode_list[index - 1].com.slave_id ), temp);
		print("Config sensor %d <id:%d> offset:%.2f\r\n", index - 1, angle_encode_list[index - 1].com.slave_id, angle_encode_list[index - 1].offset);
	}else{
		print("Config param error!\r\n");
	}
	
			
}
//单次写寄存器
int xp_angleEncode_write_reginster(Type_Angle_Encode_Def * const handler, uint16_t addr, uint16_t *data, uint8_t len)
{
    int sta = 0;
    MB_data modbus;
    uint8_t try_times = 3;

    modbus.slave_id = handler->com.slave_id;
    modbus.mb_fun = mb_single_w;
    modbus.start_addr = addr;
    modbus.wBuf = data;
    modbus.rwLen = len;
    modbus.resBuf = handler->com.rec_data;
    modbus.resLen = &handler->com.rec_len;
    while(try_times--){
        if( 0 == xp_modbus_ask(handler->com.port, &modbus) ) return 0;
    }

    return -1;
}
//保持读寄存器
int xp_angleEncode_read_reginster(Type_Angle_Encode_Def * const handler, uint16_t addr, uint16_t read_len, uint8_t *rec_data, uint8_t *rec_len)
{
    int sta = 0;
    MB_data modbus;
    uint8_t try_times = 3;


    modbus.slave_id = handler->com.slave_id;
    modbus.mb_fun = mb_hold_r;
    modbus.start_addr = addr;
    modbus.rwLen = read_len;
    modbus.resBuf = rec_data;
    modbus.resLen = rec_len;
    while(try_times--){
        if( 0 == xp_modbus_ask(handler->com.port, &modbus) ) return 0;
    }
	println("joint encode read fail !!!");
    return -1;
}

void xp_angleEncode_show_list(void)
{
	print("**************************\r\n");
	for (uint8_t i = 0; i < sizeof(angle_encode_list) / sizeof(Type_Angle_Encode_Def); i++){
		print("Sens %d <id:%d> [angle:%.2f] [offset:%.2f] [is_reverse_direct:%d]\r\n", 	i, \
																						angle_encode_list[i].com.slave_id, \
																						angle_encode_list[i].angle, \
																						angle_encode_list[i].offset, \
																						angle_encode_list[i].is_reverse_direct);
	}
	print("**************************\r\n");
}

int xp_angleEncode_debug(char *type,char *fun,char *param)
{
	//本调试接口仅支持 "enc_" 起始的命令
	if (NULL == strstr(type, "enc_")){
		return 0;
	}else{
		type += strlen("enc_");
	}
    // xp enc_listshow  打印所有编码器的相关信息
	if (strcasecmp(type, "listShow") == 0){
		xp_angleEncode_show_list();
	}
	// xp enc_setoffset 0 11.2  将第一个编码器的偏移值设置为11.2
	else if (strcasecmp(type, "setOffset") == 0){
		uint8_t index = atoi(fun);
		float offset = atof(param);
		
		if ( index < sizeof(angle_encode_list) / sizeof(Type_Angle_Encode_Def) && offset > -360 && offset < 360.0f){
			angle_encode_list[index].offset = offset;
			int32_t temp = (int32_t)(angle_encode_list[index].offset * 1000);
			xp_angleEncode_save_params(xp_angleEncode_get_offset_strKey( angle_encode_list[index].com.slave_id ), temp);
			print("Config sensor %d <id:%d> offset:%.2f\r\n", index, angle_encode_list[index].com.slave_id, angle_encode_list[index].offset);
		}else{
			print("Config param error!\r\n");
		}
	}
	// xp enc_setdirect 0 1  设置第一个编码器的旋转方向需要翻转
	else if (strcasecmp(type, "setDirect") == 0){
		uint8_t index = atoi(fun);
		uint8_t is_reverse = atoi(param);
		
		if ( index < sizeof(angle_encode_list) / sizeof(Type_Angle_Encode_Def) ){
			angle_encode_list[index].is_reverse_direct = is_reverse;
			xp_angleEncode_save_params(xp_angleEncode_get_reverse_strKey( angle_encode_list[index].com.slave_id ), angle_encode_list[index].is_reverse_direct);
			print("Config sensor %d <id:%d> is_reverse_direct:%d\r\n", index, angle_encode_list[index].com.slave_id, angle_encode_list[index].is_reverse_direct);
		}else{
			print("Config param error!\r\n");
		}
	}
	// xp enc_setangle 0 90  设置第一个编码器当前的实际角度为90度,偏移值会被计算并Flash储存
	else if (strcasecmp(type, "setAngle") == 0){
		uint8_t index = atoi(fun);
		float tar_angle = atof(param);

		xp_enc_cacl_offset(index, tar_angle);
	}
	return 1;
}

//end of the file
